#include "PosResolve.h"
#include <math.h>

using namespace cv;
using namespace std;

extern CamPos DJI_Z3_Pos; // 无人机　在靶标坐标系中的位置 (main)

//globle var:the true position of feature points
vector<Point3f> objectPoint_z3;

void cal_camera_position(Point2f *feature_points, Mat rvecsMat, Mat tvecsMat, Mat Cam_position)
{
    vector<Point2f> imagePoint_z3;
    for(int i=0; i<4; i++)
    {
        imagePoint_z3.push_back(feature_points[i]);
    }

    float Inertial_z3[9] = {14039.03667023417, 0, 3025.027507097759,
                            0, 14033.1012126011, 1947.646458303875,
                            0, 0, 1};
    /*
     * 第一次标定gazebo相机数据720P{772.5308634855991, 0, 641.8681501530496,
    0, 774.7369463871802, 353.791099492918,
    0, 0, 1};
    500w {5999.357731143755, 0, 1214.35522520652,
                            0, 5996.06580104045, 924.7808483460623,
                            0, 0, 1}
    1100w {9550.756021556441, 0, 1990.743619546634,
                            0, 9544.927339023554, 1373.303640348777,
                            0, 0, 1}


    */
    Mat camera_Matrix_z3 = Mat(3,3,CV_32FC1,Inertial_z3);

    float distCoeffs_z3[5] = {0.0282882693609772, -1.560449337946046, -0.001114958523392095, 0.0004938856803955547, 27.16908367121372};
    Mat camera_distCoeffs_z3 = Mat(1,5,CV_32FC1,distCoeffs_z3);
    /*720P {0.02342639898822997, -0.2021751514718153, -0.002350346810756892,
                              -0.001442037321494081, 0.404100768580141};
     500w {-0.04789003034441095, 4.917809395848868, -0.001918029175106872, -0.00408306857440403, -111.8650873113115}
    1100w {0.002086249693032705, -0.469338095220536, 0.0003027199318532393, -0.00185853484621265, 5.72736510287017}
    */

//    solvePnP(objectPoint_z3, imagePoint_z3, camera_Matrix_z3, camera_distCoeffs_z3, rvecsMat, tvecsMat, 0, CV_P3P );// CV_EPNP   CV_P3P
//    solvePnP(objectPoint_z3, imagePoint_z3, camera_Matrix_z3, camera_distCoeffs_z3, rvecsMat, tvecsMat, 0, CV_EPNP);
    solvePnP(objectPoint_z3, imagePoint_z3, camera_Matrix_z3, camera_distCoeffs_z3, rvecsMat, tvecsMat, 0, CV_ITERATIVE);

    // 计算 靶标坐标系下　相机的位置　＋　 Pc = R*Pw + T
    {
        Mat rotation_matrix = Mat(3,3,CV_32FC1, Scalar::all(0));  // R 旋转矩阵
        Rodrigues(rvecsMat,rotation_matrix); // 将旋转向量rvecsMat，转变成旋转矩阵rotation_matrix

        Mat M_matrix = Mat(3,4,CV_64FC1, Scalar::all(0)); // [R T] 3*4矩阵
        hconcat(rotation_matrix, tvecsMat, M_matrix);// M_matrix　为　rotation_matrix　和　tvecsMat　的水平拼接

        double extend_matrix_0001[4] = {0.0, 0.0, 0.0, 1.0};
        Mat extend_matrix = Mat(1,4,CV_64FC1, extend_matrix_0001); // [0 1] 1*4行向量

        Mat extend_M_matrix = Mat(4,4,CV_64FC1, Scalar::all(0)); // [R T][0 1] 4*4矩阵
        vconcat(M_matrix, extend_matrix, extend_M_matrix);// extend_M_matrix　为　M_matrix　和　extend_matrix　的垂直拼接

        Mat invert_extend_M_matrix = Mat(4,4,CV_64FC1, Scalar::all(0)); // 4*4矩阵的逆
        invert(extend_M_matrix, invert_extend_M_matrix, DECOMP_LU);// 矩阵求逆

        double Cam_O0001[4] = {0.0, 0.0, 0.0, 1.0}; // 相机坐标系下　相机的位置(0,0,0)
        Mat Cam_O = Mat(4,1,CV_64FC1, Cam_O0001);

        Cam_position = invert_extend_M_matrix * Cam_O/100; // 世界坐标系下 相机的位置，这里除了100，故Camposition单位是m

        // *****  获取相机的xyz(靶标坐标系)  *****
        DJI_Z3_Pos.Position_X = Cam_position.at<double>(0,0);
        DJI_Z3_Pos.Position_Y = Cam_position.at<double>(1,0);
        DJI_Z3_Pos.Position_Z = Cam_position.at<double>(2,0);

        // *****  获取相机距离靶标的距离(靶标坐标系)  *****
        double Cam_distans = sqrt(Cam_position.at<double>(0,0)*Cam_position.at<double>(0,0) +
                                  Cam_position.at<double>(1,0)*Cam_position.at<double>(1,0) +
                                  Cam_position.at<double>(2,0)*Cam_position.at<double>(2,0));
        DJI_Z3_Pos.Distance = Cam_distans;

        Mat rotation_matrix_T = Mat(3,3,CV_32FC1, Scalar::all(0));
        transpose(rotation_matrix,rotation_matrix_T); // 获取Ｒ的转置（旋转矩阵的转置　就是逆）

        Eigen::Matrix3d rotation;
        rotation <<
          rotation_matrix_T.at<double>(0,0), rotation_matrix_T.at<double>(0,1),  rotation_matrix_T.at<double>(0,2),
          rotation_matrix_T.at<double>(1,0), rotation_matrix_T.at<double>(1,1),  rotation_matrix_T.at<double>(1,2),
          rotation_matrix_T.at<double>(2,0), rotation_matrix_T.at<double>(2,1),  rotation_matrix_T.at<double>(2,2);
//        rotation <<
//          rotation_matrix.at<double>(0,0), rotation_matrix.at<double>(0,1),  rotation_matrix.at<double>(0,2),
//          rotation_matrix.at<double>(1,0), rotation_matrix.at<double>(1,1),  rotation_matrix.at<double>(1,2),
//          rotation_matrix.at<double>(2,0), rotation_matrix.at<double>(2,1),  rotation_matrix.at<double>(2,2);
        cout<<rotation<<endl;

    //    Eigen::Matrix3d F;
    //    F <<
    //         1,0,0,
    //         0,-1,0,
    //         0,0,1;
    //    Eigen::Matrix3d fixed_rot = F*rotation;
        wRo_to_euler(rotation, DJI_Z3_Pos.yaw, DJI_Z3_Pos.pitch, DJI_Z3_Pos.roll);
    }
    cout<<"rad roll:"<<DJI_Z3_Pos.roll<<" pitch:"<<DJI_Z3_Pos.pitch<<" yaw:"<<DJI_Z3_Pos.yaw<<endl;
    DJI_Z3_Pos.yaw*=180.0/3.141593;
    DJI_Z3_Pos.pitch*=180.0/3.141593;
    DJI_Z3_Pos.roll*=180.0/3.141593;

    DJI_Z3_Pos.roll = fmod(DJI_Z3_Pos.roll,360);
    DJI_Z3_Pos.pitch = fmod(DJI_Z3_Pos.pitch,360);
    DJI_Z3_Pos.yaw = fmod(DJI_Z3_Pos.yaw ,360);


}

void push_feature_points_position(int id)
{
    objectPoint_z3.clear();
    //设定目标在世界坐标系的位置   单位应该是cm
    if(id == 0)
    {
//        objectPoint_z3.push_back(Point3f(-40.0000000000,-40.0000000000,0.0000000000));
//        objectPoint_z3.push_back(Point3f(-40.0000000000,40.0000000000,0.0000000000));
//        objectPoint_z3.push_back(Point3f(40.0000000000,40.0000000000,0.0000000000));
//        objectPoint_z3.push_back(Point3f(40.0000000000,-40.0000000000,0.0000000000));
        objectPoint_z3.push_back(Point3f(-8.000000000,-8.000000000,0.0000000000));
        objectPoint_z3.push_back(Point3f(-8.000000000,8.000000000,0.0000000000));
        objectPoint_z3.push_back(Point3f(8.000000000,8.000000000,0.0000000000));
        objectPoint_z3.push_back(Point3f(8.000000000,-8.000000000,0.0000000000));

    }
    else if(id == 1)
    {
//        objectPoint_z3.push_back(Point3f(-8.0000000000,-8.0000000000,0.0000000000));
//        objectPoint_z3.push_back(Point3f(-8.0000000000,8.0000000000,0.0000000000));
//        objectPoint_z3.push_back(Point3f(8.0000000000,8.0000000000,0.0000000000));
//        objectPoint_z3.push_back(Point3f(8.0000000000,-8.0000000000,0.0000000000));
        objectPoint_z3.push_back(Point3f(-1.6000000000,-1.6000000000,0.0000000000));
        objectPoint_z3.push_back(Point3f(-1.6000000000,1.6000000000,0.0000000000));
        objectPoint_z3.push_back(Point3f(1.6000000000,1.6000000000,0.0000000000));
        objectPoint_z3.push_back(Point3f(1.6000000000,-1.6000000000,0.0000000000));
    }
    else if(id == 2)
    {
//        objectPoint_z3.push_back(Point3f(-1.6000000000,-1.6000000000,0.0000000000));
//        objectPoint_z3.push_back(Point3f(-1.6000000000,1.6000000000,0.0000000000));
//        objectPoint_z3.push_back(Point3f(1.6000000000,1.6000000000,0.0000000000));
//        objectPoint_z3.push_back(Point3f(1.6000000000,-1.6000000000,0.0000000000));
        objectPoint_z3.push_back(Point3f(-0.6400000000,-0.6400000000,0.0000000000));
        objectPoint_z3.push_back(Point3f(-0.6400000000,0.6400000000,0.0000000000));
        objectPoint_z3.push_back(Point3f(0.6400000000,0.6400000000,0.0000000000));
        objectPoint_z3.push_back(Point3f(0.6400000000,-0.6400000000,0.0000000000));
    }
}

// 旋转矩阵　转到　yaw pitch roll
void wRo_to_euler(const Eigen::Matrix3d& wRo, double& yaw, double& pitch, double& roll)
{
//    yaw = standardRad(atan2(wRo(1,0), wRo(0,0)));
//    double c = cos(yaw);
//    double s = sin(yaw);
//    pitch = standardRad(atan2(-wRo(2,0), wRo(0,0)*c + wRo(1,0)*s));
//    roll  = standardRad(atan2(wRo(0,2)*s - wRo(1,2)*c, -wRo(0,1)*s + wRo(1,1)*c));

//    yaw = atan2(wRo(1,0), wRo(0,0));
//    double c = cos(yaw);
//    double s = sin(yaw);
//    pitch = atan2(-wRo(2,0), wRo(0,0)*c + wRo(1,0)*s);
//    roll  = atan2(wRo(0,2)*s - wRo(1,2)*c, -wRo(0,1)*s + wRo(1,1)*c);

    double sy = sqrt(wRo(0,0)*wRo(0,0)+wRo(1,0)*wRo(1,0));
//    double sy2 = sqrt(wRo(2,1)*wRo(2,1)+wRo(2,2)*wRo(2,2));//两种sy结果相同
//    cout<<"sy:"<<sy<<endl;
    bool singular = sy < 1e-6;
    if(!singular)
    {
        roll = atan2(wRo(2,1),wRo(2,2));
        pitch = atan2(wRo(2,0),sy);
        yaw = atan2(wRo(1,0),wRo(0,0));
    }
    else
    {
        roll = atan2(-wRo(1,2),wRo(1,1));
        pitch = atan2(-wRo(2,0),sy);
        yaw = 0.0;
    }



}
